/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for highest common level of MaCI interfaces.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "MaCI.hpp"
#include "owndebug.h"
#include "binbag.h"
#include "MaCICtrlServer.hpp"
#include "MaCICtrlClient.hpp"
#include "AlarmServer.hpp"
#include <assert.h>

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************

//*****************************************************************************
using namespace MaCI;
//*****************************************************************************
#define MACI_REPLYQUEUE_MINOR_OFFSET 65536
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
namespace MaCI {
  std::string GetStaticClientName(const std::string &aUserKey)
  {
	char tempstr[1024];
    std::string staticname;

#ifndef WINDOWS_OS
    std::string staticnameconstruction;

    // Get hostid
    long hid = gethostid();
    sprintf(tempstr,"%x", (unsigned)hid);
    staticnameconstruction += tempstr;

    // Get UID and GID
    const unsigned int uid = getuid();
    const unsigned int euid = geteuid();
    const unsigned int gid = getgid();
    const unsigned int egid = getegid();
    sprintf(tempstr,"%u%u%u%u", uid, euid, gid, egid);
    staticnameconstruction += tempstr;

    // get current workingdir
    if (getcwd(tempstr, sizeof(tempstr))) {
      staticnameconstruction += tempstr;
    }

    // Now, blindly add the userkey
    staticnameconstruction += aUserKey;

    // Data gathering completed; now compress.
    const unsigned int keyAlen = staticnameconstruction.size() >> 1;
    const unsigned int keyBlen = staticnameconstruction.size() - keyAlen;
    const uint32_t keyA = ownHash_SimpleCRC((const unsigned char*)staticnameconstruction.c_str(), keyAlen);
    const uint32_t keyB = ownHash_SimpleCRC((const unsigned char*)staticnameconstruction.c_str()+keyAlen, keyBlen);
#else
    const uint32_t keyA = 0xF000BAA6;
    const uint32_t keyB = ownHash_SimpleCRC((const unsigned char*)aUserKey.c_str(), aUserKey.size());

#endif

    // Construct keyString.
    sprintf(tempstr, "%08x%08x", keyA, keyB);

    // Construct the final name
    staticname = "MaCIService_" + std::string(tempstr);
    assert(staticname.size() < 32);

    dPrint(ODVINFO,"Static MaCI name for this Client is: '%s'",staticname.c_str());


    return staticname;
  }
}
//*****************************************************************************

SMessageSource::SMessageSource(const gimi::GIMIMessage &aMsg)
  : gimnetID(aMsg.getSenderId()),
    majorNumber(aMsg.getMajorTypeId()),
    minorNumber(aMsg.getMinorTypeId()),
    messageID(aMsg.getMessageId())
{
}
//*****************************************************************************

SMessageSource::SMessageSource(const int aGimNetID,
                               const int aMajorNumber,
                               const int aMinorNumber,
                               const int aMessageID)
  : gimnetID(aGimNetID),
    majorNumber(aMajorNumber),
    minorNumber(aMinorNumber),
    messageID(aMessageID)
{
}
//*****************************************************************************

void SMessageSource::Print(const int level) const
{
  dPrint(level,
         "gimnetID:\t%08x\n"
         "majorNumber:\t%d\n"
         "minorNumber:\t%d\n"
         "messageID:\t%d",
         gimnetID,
         majorNumber,
         minorNumber,
         messageID);
}
//*****************************************************************************

std::string SMessageSource::ToString(bool aIncludeMajorMinor) const
{
  char formatstr[255];
  if (aIncludeMajorMinor) {
    sprintf(formatstr,"TargetID: %08X, MessageID: %u, Major: %u, Minor: %u",
            gimnetID, messageID, majorNumber, minorNumber);

  } else {
    sprintf(formatstr,"TargetID: %08X, MessageID: %u",
            gimnetID, messageID);

  }
  return std::string(formatstr);
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CMaCI::CMaCI(MaCICtrl::CMaCICtrlServer *aMaCICtrlServer,
             const int aInterfaceMajor,
             const int aInterfaceMinor,
             const std::string &aInterfaceInstanceName,
             const bool aUseStandaloneClientID)
  : iIsInternalGIMIInstance(false),
    iGIMIPtr(NULL),
    iInterfaceMajor(aInterfaceMajor),
    iInterfaceMinor(aInterfaceMinor),
    iInterfaceClientMajor( aUseStandaloneClientID ? MACI_SERVICEID2CLIENTID(aInterfaceMajor) :
                           aInterfaceMajor ),
    iInterfaceInstanceName(aInterfaceInstanceName),
    iRedirectionBufferId(0),
    iDefaultTarget(),
    iDefaultTargetMaCISL(),
    iSubscriberUnsyncedCount(-1),
    iSubscriberUnsyncedTimeout(1000),
    iStaticLinkDelay(0),
    iMaCICtrlServer(NULL),
    iMaCICtrlClient(NULL),
    iAuthenticationData(),
    iAccessData(),
    iAlarmServerPtr(NULL),
    iReplyQueueRedirectionBufferId(0),
    iIsReplyQueueEnabled(false),
    iReplyQueue(),
    iReplyQueueMutex(ownMutex_Init()),
    iReplyQueueCond(ownCond_Init()),
    iReplyQueueWatchThread(NULL),
    iUniqueMessageIDCounter(0),
    iClosing(false),
    iIsActive(false)
{
  dPrint(6,"Called Server Constructor");
  assert(aMaCICtrlServer != NULL);

  // Construct CMaCICtrlClient instance for accessing information.
  iMaCICtrlClient = new MaCICtrl::CMaCICtrlClient(iGIMIPtr, -1);
  assert(iMaCICtrlClient != NULL);

  // Assign variables.
  iMaCICtrlServer = aMaCICtrlServer;
  iGIMIPtr = aMaCICtrlServer->GetGIMIPtr();

  // Check the GIMI ptr. Must be valid on Server side.
  assert(iGIMIPtr != NULL);

  // Con
  dPrint(6,"CMaCI(Server mode) %p constructed", this);

}
//*****************************************************************************

CMaCI::CMaCI(gimi::GIMI *aGIMIPtr,
             const int aInterfaceMajor,
             const int aInterfaceMinor,
             const bool aUseStandaloneClientID)
  :  iIsInternalGIMIInstance(false),
     iGIMIPtr(aGIMIPtr),
     iInterfaceMajor(aInterfaceMajor),
     iInterfaceMinor(aInterfaceMinor),
     iInterfaceClientMajor( aUseStandaloneClientID ? MACI_SERVICEID2CLIENTID(aInterfaceMajor) :
                            aInterfaceMajor ),
     iInterfaceInstanceName(),
     iRedirectionBufferId(0),
     iDefaultTarget(),
     iDefaultTargetMaCISL(),
     iSubscriberUnsyncedCount(-1),
     iSubscriberUnsyncedTimeout(1000),
     iStaticLinkDelay(0),
     iMaCICtrlServer(NULL),
     iMaCICtrlClient(NULL),
     iAuthenticationData(),
     iAccessData(),
     iAlarmServerPtr(NULL),
     iReplyQueueRedirectionBufferId(0),
     iIsReplyQueueEnabled(false),
     iReplyQueue(),
     iReplyQueueMutex(ownMutex_Init()),
     iReplyQueueCond(ownCond_Init()),
     iReplyQueueWatchThread(NULL),
     iUniqueMessageIDCounter(0),
     iClosing(false),
     iIsActive(false)
{
  // Construct CMaCICtrlClient instance for accessing information.
  iMaCICtrlClient = new MaCICtrl::CMaCICtrlClient(iGIMIPtr, -1);
  assert(iMaCICtrlClient != NULL);
  
  // Con
  dPrint(6,"CMaCI(Client mode) %p constructed", this);
}
//*****************************************************************************

CMaCI::~CMaCI()
{
  if (IsActive()) {
    dPrintLCRed(ODTEST,"%p - WARNING: Destructing an interface which is still active!", this);
  }
  
  // Deinit.
  DisableSynchronousMessageReply();


  // Delete local copy of CMaCICtrlClient
  if (iMaCICtrlClient) {
    iMaCICtrlClient->Close();
    delete iMaCICtrlClient;
    iMaCICtrlClient = NULL;
  }

  // Delete replyqueue locks & bolts.
  ownMutex_Destroy(iReplyQueueMutex);
  iReplyQueueMutex = NULL;
  ownCond_Destroy(iReplyQueueCond);
  iReplyQueueCond = NULL;


  // Last but not least - if the internal GIMI instance is initialized.
  if (iIsInternalGIMIInstance) {
    iGIMIPtr->stop();
    delete iGIMIPtr;
    iGIMIPtr = NULL;
    iIsInternalGIMIInstance = false;
  }

  // Debug
  dPrint(ODDEBUG(1), "%p - CMaCI destructed", this);
}
//*****************************************************************************

EMaCIError CMaCI::Open()
{
  // Init Internals
  if (iRedirectionBufferId) iGIMIPtr->releaseBufferIdentifier(iRedirectionBufferId);
  iRedirectionBufferId = iGIMIPtr->reserveBufferIdentifier();

  // If MaciCtrlServer is set, attempt to open it (multiple opens dont matter)
  if (iMaCICtrlServer != NULL) iMaCICtrlServer->Open();

  // Open the local MaCICtrlClient session.
  iMaCICtrlClient->Open();

  // Call derived class DoOpen() and return its result.
  const EMaCIError result = DoOpen();

  // Update activity state if result is KMaCIOK;
  if (result == KMaCIOK){
    SetActive(true);
    iClosing = false;
  }
  return result;
}
//*****************************************************************************

EMaCIError CMaCI::Close()
{
  dPrint(ODDEBUG(1),"%p - CMaCI::Close() called", this);
  iClosing = true;
  EMaCIError result;
  //dPrint(1,"Disable");
  // Deinit internals
  DisableSynchronousMessageReply();
  //dPrint(1,"Call doclose()");
  
  // Close derived class.
  result = DoClose();

  //dPrint(1,"closed");
  dPrint(ODDEBUG(1),"%p - CMaCI::Close() completed.", this);
  // Return result of derived class DoClose().
  return result;
}
//*****************************************************************************

EMaCIError CMaCI::CreateGIMnetConnection(const std::string &aHostname,
                                         const unsigned int &aPort,
                                         const std::string &aName)
{
  EMaCIError result = KMaCIError;

  if (!iIsInternalGIMIInstance) {
    // Not internal, so override existing.
    if (iGIMIPtr) {
      dPrintLCRed(ODWARN,"Warning; overriding GIMI pointer %p at class %p", 
                  iGIMIPtr, this);
    }

    // Create new instance.
    iGIMIPtr = new gimi::GIMI();
    assert(iGIMIPtr != NULL);
    int r;

    // Now, attempt to connect using the provided credentials
    if ( (r = iGIMIPtr->connectToHubEx(aHostname, aPort, aName) ) != GIMI_OK) {
      dPrint(ODERROR, "Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
             aHostname.c_str(),
             aPort,
             aName.size()?aName.c_str():"<anonymous>",
             gimi::getGimiErrorString(r).c_str());

    } else {
      dPrint(ODVINFO, "Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
             aHostname.c_str(),
             aPort,
             aName.size()?aName.c_str():"<anonymous>",
             gimi::getGimiErrorString(r).c_str());

      // Set result.
      result = KMaCIOK;
      iIsInternalGIMIInstance = true;
    }

  } else {
    dPrint(ODERROR,"Internal GIMI already connected, will not connect again!");

  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::DestroyGIMnetConnection(void)
{
  if (iIsInternalGIMIInstance) {
    iGIMIPtr->stop();
    delete iGIMIPtr;
    iGIMIPtr = NULL;
    return KMaCIOK;
  } 
  return KMaCIError;
}
//*****************************************************************************

void CMaCI::SetDefaultTarget(const std::string &aDefaultTarget)
{
  iDefaultTarget = aDefaultTarget;
}
//****************************************************************************

bool CMaCI::SetDefaultTarget(const unsigned int aDefaultTargetId)
{
  bool result = false;

  if (iGIMIPtr) {
    result = iGIMIPtr->findClientName(iDefaultTarget,
                                      aDefaultTargetId);
  }

  return result;
}
//****************************************************************************

bool CMaCI::SetDefaultTarget(const MaCICtrl::SMaCISL &aMaCISL, const unsigned int aTimeout_ms)
{
  using namespace MaCI::MaCICtrl;
  bool result = false;
  const ownTime_ms_t begin = ownTime_get_ms();
  ownTime_ms_delta_t timeout_left = TimeoutValue(aTimeout_ms);

  dPrint(3,"Setting Default target by MaCISL: '%s'",
         aMaCISL.ToMaCISLString().c_str());

  // Open the local MaCICtrlClient session.
  iMaCICtrlClient->Open();

  // Do query for all MaCICtrl services. (Give HALF of total time, as it will ALL be consumed!)
  gimi::t_serviceList servicelist;
  if (iMaCICtrlClient->DiscoverMaCICtrlServices(servicelist, timeout_left - (timeout_left/4)) == KMaCIOK) {

    // Update timeout left.
    timeout_left = ownTime_get_ms_left(timeout_left, begin);

    if (timeout_left > 0) {
      // Now, call the 'next' version of SetDefault target.
      result = SetDefaultTarget(aMaCISL, servicelist, timeout_left);
    }

  } else {
    dPrint(2,"Failed to Query for MaCICtrl compliant services!");

  }

  // Return with result.
  return result;
}
//*****************************************************************************

bool CMaCI::SetDefaultTarget(const MaCICtrl::SMaCISL &aMaCISL,
                             const gimi::t_serviceList &aServiceList,
                             const unsigned int aTimeout_ms)
{
  using namespace MaCI::MaCICtrl;
  bool result = false;
  ownTime_ms_delta_t timeout_left = TimeoutValue(aTimeout_ms);

  // (Re)Open the local MaCICtrlClient session.
  iMaCICtrlClient->Open();

  TServiceEntryArray entries;
  if (timeout_left > 0) {
    MaCI::EMaCIError me = iMaCICtrlClient->GetServiceArray(entries, aServiceList, timeout_left);
    if (me == MaCI::KMaCIOK) {
      result = SetDefaultTarget(aMaCISL, entries);

    } else {
      dPrint(ODINFO, "Failed to query information for Entries! (%s)",
             MaCI::GetErrorStr(me).c_str());

    }
  } else {
    dPrint(ODINFO,"No time left to set default target (aTimeout_ms = %u)",
           aTimeout_ms);

  }
  return result;
}
//****************************************************************************

bool CMaCI::SetDefaultTarget(const MaCICtrl::SMaCISL &aMaCISL,
                             const MaCICtrl::TServiceEntryArray &aServices)
{
  using namespace MaCI::MaCICtrl;
  bool result = false;
  MaCI::EMaCIError me;

  SServiceEntry se;
  if (( me = iMaCICtrlClient->FindService(se, aServices, aMaCISL) ) == KMaCIOK) {

    // Warning?
    if ((int)se.service.servicemajor != iInterfaceMajor) {
      dPrint(1,"WARNING: Requested MaCISL points to service with different Major! (remote:%u != local:%u)",
             se.service.servicemajor, iInterfaceMajor);
    }

    // Set default target & minor!
    dPrint(3, "Service located - changing default targetName to '%s', using minor %u",
           se.gimnetname.c_str(),
           se.service.serviceminor);

    // Set; default target, interfaceMinor and local MaCISL.
    SetDefaultTarget(se.gimnetname);
    iInterfaceMinor = se.service.serviceminor;
    iDefaultTargetMaCISL = aMaCISL;
    result = true;

  } else {
    dPrint(3,"Failed to Find service with MaCISL '%s'!",
           aMaCISL.ToMaCISLString().c_str());

  }
  return result;
}
//****************************************************************************

void CMaCI::SetStaticLinkDelay(const unsigned int aDelay_ms)
{
  iStaticLinkDelay = aDelay_ms;
}
//****************************************************************************

bool CMaCI::SetGroupName(const std::string &aGroupName)
{
  bool result = false;
  if (iMaCICtrlServer) result = iMaCICtrlServer->SetGroupName(aGroupName);
  return result;
}
//*****************************************************************************

const std::string & CMaCI::GetDefaultTarget(void) const
{
  return iDefaultTarget;
}
//****************************************************************************

const MaCICtrl::SMaCISL & CMaCI::GetDefaultTargetMaCISL(void) const
{
  return iDefaultTargetMaCISL;
}
//****************************************************************************

int CMaCI::GetInterfaceSubscriberCount(void) const {
  int result = -1;
  if (iGIMIPtr) {
    const int v = iGIMIPtr->getSubscriberCount(iInterfaceMajor, iInterfaceMinor);
    if (v >= 0) {
      // If GIMI returned valid value... (which is; greater than or equal to Zero)
      result = v;
    }
  }
  return result;
}
//****************************************************************************

MaCICtrl::CMaCICtrlServer *CMaCI::GetMaCICtrlServerPtr(void) const
{
  return iMaCICtrlServer;
}
//****************************************************************************

gimi::GIMI *CMaCI::GetGIMIPtr(void) const
{
  return iGIMIPtr;
}
//****************************************************************************

std::string CMaCI::GetInterfaceName(void) const
{
  std::string result("<Unknown>");

  // Unconditional call.
  gimi::datatypeDefinitions.getDatatypeName(result,
                                            iInterfaceMajor);

  return result;
}
//****************************************************************************

std::string CMaCI::GetInterfaceInstanceName(void) const
{
  return iInterfaceInstanceName;
}
//****************************************************************************

std::string CMaCI::GetGroupName(void) const
{
  std::string result;
  if (iMaCICtrlServer) {
    result = iMaCICtrlServer->GetGroupName();
  }
  return result;
}
//****************************************************************************

const unsigned int CMaCI::GetStaticLinkDelay(void) const
{
  return iStaticLinkDelay;
}
//****************************************************************************

bool CMaCI::IsGIMnetConnectionValid(void) const
{
  bool result = false;
  if (iGIMIPtr) {
    result = iGIMIPtr->isConnected();
  }
  return result;
}
//****************************************************************************

EMaCIError CMaCI::EnableAlarmServer(void)
{
  using namespace MaCI::Alarm;
  EMaCIError result = KMaCIError;
  if (iAlarmServerPtr == NULL && iMaCICtrlServer != NULL) {
    // Construct minor by combining current interfaces Major and Minor numbers.
    const unsigned int as_minor = ((iInterfaceMajor & 0xFFFF) << 16) | (iInterfaceMinor & 0xFFFF);

    // Create new instance.
    iAlarmServerPtr = new CAlarmServer(iMaCICtrlServer,
                                       as_minor,
                                       std::string("MaCI Integrated MaCI::Alarm interface"),
                                       GetInterfaceName() + "+" + GetInterfaceInstanceName());
    assert(iAlarmServerPtr != NULL);

    // Open.
    result = iAlarmServerPtr->Open();
  }
  return result;
}
//****************************************************************************

EMaCIError CMaCI::DisableAlarmServer(void)
{
  EMaCIError result = KMaCIError;
  if (iAlarmServerPtr != NULL) {
    // Just call delete - will take care of cleaning up.
    delete iAlarmServerPtr;

    // Mark pointer NULL to allow reconstruction.
    iAlarmServerPtr = NULL;

    // Operation was succesfull.
    result = KMaCIOK;
  }
  return result;
}
//****************************************************************************
//****************************************************************************
//****************************************************************************
//****************************************************************************
//****************************************************************************

bool CMaCI::RegisterProvidedService(const std::string &aDescription)
{
  bool r, result = false;

  // If interfaceminor is negative, do auto-search of free major/minor pair.
  if (iInterfaceMinor < 0) {
    // Start from Zero.
    iInterfaceMinor = 0;

    // Find a 'Free' TypeID pair.
    while( iGIMIPtr->isFreeTypeId(iInterfaceMajor, iInterfaceMinor) == false)
      ++iInterfaceMinor;

    // Debug
    dPrint(8,"Auto-probing Minor for Major %d resulted in minor %d",
           iInterfaceMajor, iInterfaceMinor);
  }

  // Register the interface.
  r = iGIMIPtr->addProvidedService(iInterfaceMajor,
                                   iInterfaceMinor,
                                   aDescription);
  if (r) {
    dPrint(6,"Succesfully registered providedService '%d:%d'",
           iInterfaceMajor, iInterfaceMinor);
    result = true;

  } else {
    dPrint(6,"ProvidedService %d:%d already exists.",
           iInterfaceMajor, iInterfaceMinor);

  }

  // Register to MaCICtrlServer as service if it is Set (in Server Mode)
  if (result && iMaCICtrlServer) {
    iMaCICtrlServer->AddGroupService(iInterfaceMajor, iInterfaceMinor,
                                     MaCICtrl::KServiceTypeProvided,
                                     iInterfaceInstanceName);

    dPrint(6,"Added Provided-Service %d:%d to MaCICtrlGroup '%s', Instance is: '%s'",
           iInterfaceMajor, iInterfaceMinor,
           iMaCICtrlServer->GetGroupName().c_str(),
           iInterfaceInstanceName.c_str());

  }

  return result;
}
//****************************************************************************

bool CMaCI::UnregisterProvidedService(void)
{
  bool r, result = false;

  r = iGIMIPtr->removeProvidedService(iInterfaceMajor,
                                      iInterfaceMinor);

  if (r) {
    dPrint(6,"Succesfully unregistered providedService '%d:%d'",
           iInterfaceMajor, iInterfaceMinor);
    result = true;

  } else {
    dPrint(6,"ProvidedService '%d:%d' doesn't exist (Hence not unregistered)",
           iInterfaceMajor, iInterfaceMinor);

  }

  // Remove from MaCICtrlServer if it is set (In Server mode)
  if (result && iMaCICtrlServer) {
    iMaCICtrlServer->RemoveGroupService(iInterfaceMajor, iInterfaceMinor,
                                        MaCICtrl::KServiceTypeProvided);

    dPrint(6,"Removed Provided-Service %d:%d from MaCICtrlGroup '%s'",
           iInterfaceMajor, iInterfaceMinor,
           iMaCICtrlServer->GetGroupName().c_str());
  }

  return result;
}
//****************************************************************************

bool CMaCI::RegisterAcceptedService(const std::string aDescription)
{
  bool r, result = false;

  // If interfaceMinor is negative, do autoprobing.
  if (iInterfaceMinor < 0) {
    // Start from Zero.
    iInterfaceMinor = 0;

    // Find a 'Free' TypeID pair.
    while( iGIMIPtr->isFreeTypeId(iInterfaceMajor, iInterfaceMinor) == false)
      ++iInterfaceMinor;

    // Debug
    dPrint(8,"Auto-probing Minor for Major %d resulted in minor %d",
           iInterfaceMajor, iInterfaceMinor);
  }

  // Add Accepted service
  r = iGIMIPtr->addAcceptedService(iInterfaceMajor,
                                   iInterfaceMinor,
                                   aDescription);

  // Add Redirection
  r &= iGIMIPtr->addRedirection(iRedirectionBufferId,
                                iInterfaceMajor,
                                iInterfaceMinor,
                                iDefaultTarget);
  if (r) {
    dPrint(6,"Succesfully registered acceptedService '%d:%d'",
           iInterfaceMajor, iInterfaceMinor);
    result = true;

  } else {
    dPrint(6,"AcceptedService %d:%d already exists.",
           iInterfaceMajor, iInterfaceMinor);

  }

  // If In server mode, add to Group.
  if (result && iMaCICtrlServer) {
    iMaCICtrlServer->AddGroupService(iInterfaceMajor, iInterfaceMinor,
                                     MaCICtrl::KServiceTypeAccepted,
                                     iInterfaceInstanceName);

    dPrint(6,"Added Accepted-Service %d:%d to MaCICtrlGroup '%s', Instance is: '%s'",
           iInterfaceMajor, iInterfaceMinor,
           iMaCICtrlServer->GetGroupName().c_str(),
           iInterfaceInstanceName.c_str());

  }

  return result;
}
//****************************************************************************

bool CMaCI::UnregisterAcceptedService()
{
  bool r, result = false;

  // Remove accepted service
  r = iGIMIPtr->removeAcceptedService(iInterfaceMajor,
                                      iInterfaceMinor);

  // Remove redirection
  r &= iGIMIPtr->removeRedirection(iRedirectionBufferId,
                                   iInterfaceMajor,
                                   iInterfaceMinor,
                                   iDefaultTarget);
  if (r) {
    dPrint(6,"Succesfully unregistered acceptedService '%d:%d'",
           iInterfaceMajor, iInterfaceMinor);
    result = true;

  } else {
    dPrint(6,"AcceptedService '%d:%d' doesn't exist (Hence not unregistered)",
           iInterfaceMajor, iInterfaceMinor);

  }

  // IF in server mode, remove from group.
  if (result && iMaCICtrlServer) {
    iMaCICtrlServer->RemoveGroupService(iInterfaceMajor, iInterfaceMinor,
                                        MaCICtrl::KServiceTypeAccepted);

    dPrint(6,"Removed Accepted-Service %d:%d from MaCICtrlGroup '%s'",
           iInterfaceMajor, iInterfaceMinor,
           iMaCICtrlServer->GetGroupName().c_str());

  }


  return result;
}
//****************************************************************************

EMaCIError CMaCI::ReceiveInterfaceMessage(gimi::GIMIMessage &aMsg,
                                          const int aTimeout_ms)
{
  assert(this != NULL);

  EMaCIError result = KMaCIError;
  int r;

  r = iGIMIPtr->receive(aMsg,
                        aTimeout_ms,
                        iInterfaceMajor,
                        iInterfaceMinor,
                        iRedirectionBufferId);
  switch(r) {
  case GIMI_OK:
    // Got GIMI OK, so mark operation succesfull.
    result = KMaCIOK;
    break;

  case GIMI_TIMEOUT:
  case GIMI_NOMSGS:
    result = KMaCITimeout;
    /*    dPrint(10,"GIMI Timeout. No messages for %d:%d - %d",
          iInterfaceMajor, iInterfaceMinor, iRedirectionBufferId);*/
    break;

  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    if(!iClosing){
      // Any error, print warning and sleep for one second.
      dPrint(ODWARN,"GIMI Error (%d, '%s') received!",
             r, gimi::getGimiErrorString(r).c_str());
    }
      break;
  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::SendInterfaceMessage(const char *aBuffer,
                                       const int aBufferSize,
                                       const unsigned int aMessageID)
{
  assert(this != NULL);

  EMaCIError result = KMaCIError;
  int r;

  /*  dPrint(10,"Sending message to '%s' %d:%d",
         iDefaultTarget.c_str(),
         iInterfaceMajor,
         iInterfaceMinor);*/

  // Send the message to default target with default major & minor numbers.
  r = iGIMIPtr->send(iDefaultTarget,
                     aBuffer,
                     aBufferSize,
                     iInterfaceMajor,
                     iInterfaceMinor,
                     aMessageID);
  switch(r) {
  case GIMI_OK:
    // Message sent OK.
    result = KMaCIOK;
    break;

  case GIMI_NOCONNECTION:
    result = KMaCINoConnection;
    break;

  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());
    break;
  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::SendInterfaceMessage(const CMaCIData &aData,
                                       const unsigned int aMessageId)
{
  EMaCIError result = KMaCIError;

  // Get pointer to BinBag container.
  const gim::binbag::CBinBag *data = aData.GetBinBagContainerPtr();
  if (data) {
    // If pointer was valid, take the data and send!
    result = SendInterfaceMessage(data->GetBuffer(),
                                  data->GetBufferSize(),
                                  aMessageId);
  }
  return result;
}
//*****************************************************************************

EMaCIError CMaCI::SendInterfaceSubscriberMessage(const char *aBuffer,
                                                 const int aBufferSize,
                                                 const unsigned int aMessageID)
{
  assert(this != NULL);


  EMaCIError result = KMaCIError;
  int r;

  // Send the message to default target with default major & minor numbers.
  r = iGIMIPtr->sendToSubscribers(aBuffer,
                                  aBufferSize,
                                  iInterfaceMajor,
                                  iInterfaceMinor,
                                  aMessageID,
                                  0, // MessageFlags
                                  iSubscriberUnsyncedCount, // Unsync MAX
                                  iSubscriberUnsyncedTimeout); // Unsync timeout

  switch(r) {
  case GIMI_OK:
  case GIMI_NOSUBSCRIBERS:
    // Message sent OK, or atleast no communication error occured.
    result = KMaCIOK;
    break;

  case GIMI_NOCONNECTION:
    result = KMaCINoConnection;
    break;

  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(4,"GIMI Error (%d, '%s') - from send() received (major %u, minor %u)",
           r, gimi::getGimiErrorString(r).c_str(), iInterfaceMajor, iInterfaceMinor);
    break;
  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::SendSynchronousMessageReply(const char *aBuffer, const int aBufferSize,
                                              const int aTargetID,
                                              const int aMajorNumber,
                                              const int aMinorNumber,
                                              const unsigned int aMessageID)
{
  EMaCIError result = KMaCIError;

  // Send the message as reply
  int r = iGIMIPtr->send(aTargetID,
                         aBuffer,
                         aBufferSize,
                         aMajorNumber,
                         aMinorNumber + MACI_REPLYQUEUE_MINOR_OFFSET,
                         aMessageID);
  switch(r) {
  case GIMI_OK:
    // Message sent OK.
    result = KMaCIOK;
    break;

  case GIMI_NOCONNECTION:
    result = KMaCINoConnection;
    break;

  default:
    break;
  }

  return result;
}
//*****************************************************************************

void CMaCI::SetInterfaceSubscriberSyncLimits(const int aUnsyncCount,
                                             const int aUnsyncTimeout)
{
  iSubscriberUnsyncedCount = aUnsyncCount;
  iSubscriberUnsyncedTimeout = aUnsyncTimeout;
}
//*****************************************************************************

EMaCIError CMaCI::SendSynchronousMessageReply(const char *aBuffer, const int aBufferSize,
                                              const SMessageSource &aSource)
{
  return SendSynchronousMessageReply(aBuffer,
                                     aBufferSize,
                                     aSource.gimnetID,
                                     aSource.majorNumber,
                                     aSource.minorNumber,
                                     aSource.messageID);
}
//*****************************************************************************

EMaCIError CMaCI::ReceiveSynchronousMessageReply(gimi::GIMIMessage &aMsg,
                                                 const int aMessageId,
                                                 const int aTimeout_ms)
{
  EMaCIError e;
  std::vector<gimi::GIMIMessage *> gmv;
  std::vector<int> iv;

  // Add one.
  iv.push_back(aMessageId);

  // call vector model. This function is only a special case of that.
  e = ReceiveSynchronousMessageReply(gmv, iv, aTimeout_ms);

  // If result was OK, store the first given value to caller.
  if (e == KMaCIOK) {
    gimi::GIMIMessage *msgp = gmv[0];
    assert(msgp != NULL);
    aMsg.assign(*msgp);
    delete msgp;
  }

  return e;
}
//*****************************************************************************

EMaCIError CMaCI::ReceiveSynchronousMessageReply(std::vector<gimi::GIMIMessage *> &aGIMIMessageArray,
                                                 const std::vector<int> &aMessageIDArray,
                                                 const int aTimeout_ms)
{
  EMaCIError result = KMaCIError;


  // Construct copy of vector. This is decremented on each call.
  std::vector<int> searchVector = aMessageIDArray;
  gimi::GIMIMessage gmsg;

  // Clear (verify)
  aGIMIMessageArray.clear();

  // Times
  ownTime_ms_t tbegin = ownTime_get_ms();
  ownTime_ms_delta_t tleft = aTimeout_ms;

  // Lock
  LockReplyQueue();

  // Run while; 1) not all ID:s in array received, 2) have not timed out.
  do {
    // Message queue check
    for(EACH_IN_i(searchVector)) {
      if (*i >= 0) {
        if (SearchReplyQueue(gmsg, *i)) {
          // The wanted reply was found from Queue!
          // Zero the entry in queue.
          *i = -1;

          // gmsg
          gimi::GIMIMessage *nmsg = new gimi::GIMIMessage;
          nmsg->assign(gmsg);
          aGIMIMessageArray.push_back(nmsg);
        }
      }
    }

    // Don't have all requested replies?
    if (aGIMIMessageArray.size() != aMessageIDArray.size()) {
      // Time check
      tleft = ownTime_get_ms_left(aTimeout_ms, tbegin);
      // Then Wait, for a maximum time of time left from specified timeout.
      if (tleft > 0) {
        WaitReplyQueue(tleft);

      } else {
        result = KMaCITimeout;
        break; // while()
      }

    } else {
      // Got all messages, mark OK & break
      result = KMaCIOK;
      break;
    }
  } while(true);

  UnlockReplyQueue();

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::EnableSynchronousMessageReply(bool aAcceptBroadcasts)
{
  EMaCIError result = KMaCIError;

  assert(iGIMIPtr != NULL);

  LockReplyQueue();
  if (!iIsReplyQueueEnabled) {
    // Mark queue enabled.
    iIsReplyQueueEnabled = true;

    // Create redir
    iReplyQueueRedirectionBufferId = iGIMIPtr->reserveBufferIdentifier();

    // Add Accepted service
    iGIMIPtr->addAcceptedService(iInterfaceMajor,
                                 iInterfaceMinor + MACI_REPLYQUEUE_MINOR_OFFSET,
                                 "ReplyQueue");

    // Add Redirection. If AcceptBroadcasts is 'true', don't set bound
    // target.
    iGIMIPtr->addRedirection(iReplyQueueRedirectionBufferId,
                             iInterfaceMajor,
                             iInterfaceMinor + MACI_REPLYQUEUE_MINOR_OFFSET,
                             aAcceptBroadcasts ? "" : iDefaultTarget);


    // Start handler.
    assert(iReplyQueueWatchThread == NULL);
    iReplyQueueWatchThread = ownThread_Create((void*)ReplyQueueWatchThread, this);

    // Result
    result = KMaCIOK;
  }
  UnlockReplyQueue();

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::DisableSynchronousMessageReply(void)
{
  EMaCIError result = KMaCIError;

  assert(iGIMIPtr != NULL);

  LockReplyQueue();
  if (iIsReplyQueueEnabled) {
    // Mark queue disabled.
    iIsReplyQueueEnabled = false;

    // Stop thread.
    //ownThread_Cancel(iReplyQueueWatchThread);
    assert(iReplyQueueWatchThread != NULL);

    NotifyReplyQueue();
    NotifyReplyQueue();
    
    // Remove accepted service
    iGIMIPtr->removeAcceptedService(iInterfaceMajor,
                                    iInterfaceMinor + MACI_REPLYQUEUE_MINOR_OFFSET);
    
    // Remove redirection
    iGIMIPtr->removeRedirection(iReplyQueueRedirectionBufferId,
                                iInterfaceMajor,
                                iInterfaceMinor + MACI_REPLYQUEUE_MINOR_OFFSET,
                                iDefaultTarget);
    
    // Release bufferidentifier
    iGIMIPtr->releaseBufferIdentifier(iReplyQueueRedirectionBufferId);
    iReplyQueueRedirectionBufferId = 0;
    

    ownThread_Join(iReplyQueueWatchThread, NULL);
    iReplyQueueWatchThread = NULL;

    // Clear the queue.
    for(EACH_IN_i(iReplyQueue)) {
      delete i->second;
    }
    iReplyQueue.clear();
    
    
    
    // Result
    result = KMaCIOK;
  }
  UnlockReplyQueue();

  return result;
}
//*****************************************************************************

bool CMaCI::IsSynchronousMessageReplyEnabled(void) const
{
  return iIsReplyQueueEnabled;
}
//*****************************************************************************

int CMaCI::ReplyQueueWatchThread(CMaCI *that)
{
  gimi::GIMIMessage rmsg;
  const unsigned int timeout = 1000*60*60*24;

  while(that->iIsReplyQueueEnabled) {

    int r = that->iGIMIPtr->receive(rmsg,
                                    timeout,
                                    that->iInterfaceMajor,
                                    that->iInterfaceMinor + MACI_REPLYQUEUE_MINOR_OFFSET,
                                    that->iReplyQueueRedirectionBufferId);

    if (r == GIMI_OK) {
      // Got message. Check and push to Queue if not the right one.
      gimi::GIMIMessage *nmsg = new gimi::GIMIMessage;
      nmsg->assign(rmsg);

      that->LockReplyQueue();
      that->iReplyQueue.insert(std::pair<int, gimi::GIMIMessage *>(nmsg->getMessageId(), nmsg));
      that->UnlockReplyQueue();
      that->NotifyReplyQueue();
            

    } else if (r == GIMI_TIMEOUT || r == GIMI_NOMSGS) {
      // No messages received. Timed out. Do nada special

    } else if (r == GIMI_INTERRUPT) {
      // Any error, print warning and sleep for one second.
      if(!that->iClosing){
        dPrint(ODTEST,"GIMI Error (%d, '%s') received!",
               r, gimi::getGimiErrorString(r).c_str());
        if(that->iIsReplyQueueEnabled){
          ownSleep_ms(1000);
        }
      }

    } else {
      if(!that->iClosing){
        dPrint(ODTEST,"GIMI Error (%d, '%s') received!",
               r, gimi::getGimiErrorString(r).c_str());
      }
    }
  }

  return 0;
}
//*****************************************************************************

EMaCIError CMaCI::SubscribeToInterface(unsigned int aTimeout_ms,
                                       unsigned int aResubscribe_ms)
{
  EMaCIError result = KMaCIError;
  int r;

  assert(iGIMIPtr != NULL);

  // Replace assert with ugly print + error return.
  if (iInterfaceMinor < 0) {
    dPrintLCRed(ODERROR,"Attempting to Subscribe to service with negative minor ('%s':%d is invalid) - Target not set?", 
                iDefaultTarget.c_str(),
                iInterfaceMinor);
    return KMaCIError;
  }


  // Add Accepted service
  r = iGIMIPtr->addAcceptedService(iInterfaceMajor,
                                   iInterfaceMinor,
                                   "ServiceData Input");
  // Add Redirection
  r &= iGIMIPtr->addRedirection(iRedirectionBufferId,
                                iInterfaceMajor,
                                iInterfaceMinor,
                                iDefaultTarget);
  if (r) {
    dPrint(6,"Succesfully registered acceptedService '%d:%d'",
           iInterfaceMajor, iInterfaceMinor);

  } else {
    dPrint(6,"AcceptedService %d:%d already exists.",
           iInterfaceMajor, iInterfaceMinor);

  }

  // Attempt to subscribe to service.
  r = iGIMIPtr->subscribeToService(iDefaultTarget,
                                   iInterfaceMajor,
                                   iInterfaceMinor,
                                   TimeoutValue(aTimeout_ms),
                                   aResubscribe_ms);
  switch(r) {
  case GIMI_OK:
  case GIMI_INVALIDCLIENT:
    dPrint(3,"Succesfully Subscribed to Service (%d:%d) at target '%s' : '%s'",
           iInterfaceMajor,
           iInterfaceMinor,
           iDefaultTarget.c_str(),
           gimi::getGimiErrorString(r).c_str());
    result = KMaCIOK;
    break;

  case GIMI_TIMEOUT:
    if (result == KMaCIError) result = KMaCITimeout;
  case GIMI_INVALIDTYPE:
    if (result == KMaCIError) result = KMaCITimeout;
  case GIMI_NOCONNECTION:
    if (result == KMaCIError) result = KMaCINoConnection;
  case GIMI_ERROR:
  default:
    dPrint(2,"Failed to Subscribe to Service (%d:%d) at target '%s' : '%s'",
           iInterfaceMajor,
           iInterfaceMinor,
           iDefaultTarget.c_str(),
           gimi::getGimiErrorString(r).c_str());
    break;
  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::UnsubscribeToInterface(const unsigned int aTimeout_ms)
{
  EMaCIError result = KMaCIError;

  assert(iGIMIPtr != NULL);
  //Check how many redirection buffer are added
  gimi::t_redirectionList redirectionList;
  int r = GIMI_ERROR;
  int i = iGIMIPtr->listRedirections(redirectionList,
                                     false,
                                     iInterfaceMajor,
                                     iInterfaceMinor);

  // Remove our own redirection.
  const bool rs = iGIMIPtr->removeRedirection(iRedirectionBufferId,
                                              iInterfaceMajor,
                                              iInterfaceMinor,
                                              iDefaultTarget);

  // Release buffer identifier
  if (iRedirectionBufferId) {

    iGIMIPtr->releaseBufferIdentifier(iRedirectionBufferId);
    iRedirectionBufferId = 0;
  }

  if(i < 2){
    // If there's only one buffer, only one client is subscribed
    // to the same major/minor => we can unsubscribe from service.

    r = iGIMIPtr->unsubscribeService(iDefaultTarget,
                                     iInterfaceMajor,
                                     iInterfaceMinor,
                                     TimeoutValue(aTimeout_ms));

    // Remove accepted service.
    r &= iGIMIPtr->removeAcceptedService(iInterfaceMajor,
					 iInterfaceMinor);
  }else{
    // No unsubscribing because there's multiple buffers left
    r = GIMI_OK;

  }

  // Check values.
  if (r == GIMI_OK && rs) {
    dPrint(8,"Succesfully unsubscribed from service '%d:%d' on '%s'",
           iInterfaceMajor, iInterfaceMinor, iDefaultTarget.c_str());
    result = KMaCIOK;

  } else {
    dPrint(3,"Failed to unsubscribe from service '%d:%d' on '%s'",
           iInterfaceMajor, iInterfaceMinor, iDefaultTarget.c_str());

  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::IsServiceAcceptedOnTarget(const unsigned int &aTimeout_ms)
{
  EMaCIError result = KMaCIError;
  int r;
  gimi::t_serviceList serviceList;

  r = iGIMIPtr->serviceDiscoveryToTarget(serviceList,
                                         iDefaultTarget,
                                         TimeoutValue(aTimeout_ms));  ///1s+delay timeout
  if (r != GIMI_OK) {
    // GIMI returned error?
    switch(r) {
    case GIMI_TIMEOUT:
      result = KMaCITimeout;
      break;
    case GIMI_INVALIDCLIENT:
      result = KMaCIUnknownClient;
      break;
    case GIMI_NOCONNECTION:
      result = KMaCINoConnection;
      break;
    default:
      break;
    }
    
  } else {
    //GIMI returned OK
    for (gimi::t_serviceListConstIterator iter = serviceList.begin();
         iter != serviceList.end();
         ++iter) {
      
      if (iter->serviceType == GIMI_SERVICETYPE_ACCEPTED &&
          iter->majorServiceId == iInterfaceMajor &&
          iter->minorServiceId == iInterfaceMinor) {
        //        dPrint(ODTEST,"Required ACCEPTED service found. (%d:%d)",
        //       iInterfaceMajor, iInterfaceMinor);
        
        result = KMaCIOK;
        break;
      }
    }
    
    if(result != KMaCIOK) {
      dPrint(ODTEST,"No required ACCEPTED service found (%d:%d), from %s",
             iInterfaceMajor, iInterfaceMinor, iDefaultTarget.c_str());
      
    }
  }

  return result;
}
//*****************************************************************************

EMaCIError CMaCI::GetInterfaceAccess(MaCICtrl::TAccess &aAccess,
                                     const unsigned int aNodeID)
{
  EMaCIError result = KMaCIOK;

  MaCICtrl::CMaCICtrlServer *ms = GetMaCICtrlServer();

  ms->GetAccessInfo(aAccess,
                    iInterfaceMajor,
                    iInterfaceMinor,
                    aNodeID,
                    true);

  return result;
}
//*****************************************************************************

bool CMaCI::IsInterfaceAccess(const unsigned int aNodeID)
{
  MaCICtrl::TAccess a;
  MaCICtrl::CMaCICtrlServer *mcs = GetMaCICtrlServer();

  if (mcs != NULL) {
    mcs->GetAccessInfo(a,
                       iInterfaceMajor,
                       iInterfaceMinor,
                       aNodeID,
                       true);
  } else {
    dPrint(2,"Unable to Check Interface Access - No MaCICtrlServer set!");

  }

  return a.IsAccess();
}
//*****************************************************************************

void CMaCI::SetServerInterfaceAuthentication(const MaCICtrl::TAuthentication &aAuth,
                                             const int aMaxAccess,
                                             const int aAccessTimeout_ms)
{
  MaCICtrl::CMaCICtrlServer *mcs = GetMaCICtrlServer();
  if (mcs != NULL) {
    mcs->SetAccessKey(iInterfaceMajor,
                      iInterfaceMinor,
                      aAuth,
                      aMaxAccess,
                      aAccessTimeout_ms);
  } else {
    dPrint(2,"Unable to Set Access Key - No MaCICtrlServer set!");

  }
}
//*****************************************************************************

void CMaCI::SetClientInterfaceAuthentication(const MaCICtrl::TAuthentication &aAuth)
{
  // Store the given authentication inside class instance.
  iAuthenticationData = aAuth;
}
//*****************************************************************************

EMaCIError CMaCI::ValidateClientInterfaceAuthentication(MaCICtrl::TAccess &aAccess)
{
  EMaCIError result = KMaCIError;
  const MaCICtrl::CMaCICtrlClient *mc = GetMaCICtrlClient();

  // Make request.
  result = mc->RequestAuthentication(iDefaultTarget,
                                     iInterfaceMajor,
                                     iInterfaceMinor,
                                     iAuthenticationData,
                                     iAccessData,
                                     0, TimeoutValue(2000));

  // If call was succesfull, copy the access information too.
  if (result == KMaCIOK) {
    aAccess = iAccessData;
  }

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

bool CMaCI::SearchReplyQueue(gimi::GIMIMessage &aMsg, const int aMessageID)
{

  // \todo Implement keying by other fields too (like; sender)
  bool result = false;
  // Search the replyQueue for existing entry
  if (!iReplyQueue.empty()) {
    ITERATOR(iReplyQueue) iter = iReplyQueue.find(aMessageID);
    if (iter != iReplyQueue.end()) {
      // So we have a reply!
      result = true;

      // Assign data from map
      aMsg.assign(*iter->second);

      // Remove entry from map
      delete iter->second;
      iReplyQueue.erase(iter);

    }
  }

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
